﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.ComponentModel;
using System.Net;

namespace Localizer
{
    class Localizer
    {
        #region Memeber Variables 
        private BackgroundWorker _locationWorker;
        private int _timeStep;
        private IPAddress _quadIP;
        private List<IPAddress> _receiverIP;
        private Queue<Point> _wayPoints;
        private Point _currentLocation;
        private Point _priorLocation;

        #endregion

        #region Constructor
        public Localizer(int timeStep, IPAddress quadIP, List<IPAddress> receiverIP)
        {
            this._timeStep = timeStep;
            this._quadIP = quadIP;
            this._receiverIP = receiverIP;
            this._wayPoints = new Queue<Point>(100);

            this._locationWorker = new BackgroundWorker();
            this._locationWorker.DoWork += new DoWorkEventHandler(locationWorker_DoWork);

            this._locationWorker.RunWorkerAsync();
            
        }

        public Localizer() : this(100, new IPAddress(new byte[] {10,0,0,100}), new List<IPAddress>())
        {
            
        }
        #endregion

        private void locationWorker_DoWork(object sender, DoWorkEventArgs e)
        {
            long cycleCount = 0;

            while (!e.Cancel)
            {
                if (cycleCount % 2 == 0) // Called every 20[ms]
                    sendPacket();
                if (cycleCount % 10 == 0) // Called every 100[ms]
                    pingUS();
                if(cycleCount % 100 == 0) // Called every 1000[ms]
                    updateLocation();
                if(cycleCount % 500 == 0) // Called every 5000[ms]
                    updateHeading();

                    System.Threading.Thread.Sleep(10);
                cycleCount++;
            }
        }

        private void sendPacket()
        {
        }

        private void pingUS()
        {
            throw new NotImplementedException("Need to implement pingUS");
        }

        private void updateLocation()
        {
            if (_wayPoints.Count > 0)
                _currentLocation = _wayPoints.Dequeue();
            else
                _currentLocation = _priorLocation;

            /* Do pid here */
            // coptorElevatorValue = xPid.Compute(_currentLocation.X);
            // coptorAileronValue = yPid.Compute(_currentLocation.Y);
            // coptorThrottleValue = zPid.Compute(_currentLocation.Z);

            // Send Coptor Values

            _priorLocation = _currentLocation;
        }

        private void updateHeading()
        {
            
            throw new NotImplementedException("Need to implement updateHeading");

            if (_wayPoints.Count > 0)
                _wayPoints.Dequeue();
        }
    }

    class Point
    {
        private double _x;
        private double _y;
        private double _z;

        public Point(double X, double Y, double Z)
        {
            this._x = X;
            this._y = Y;
            this._z = Z;
        }

        public Point() : this(0.0, 0.0, 0.0){}

        public double X { get{return this._x;} set{_x = value;} }
        public double Y { get{return this._y;} set{_y = value;} }
        public double Z { get{return this._z;} set{_z = value;} }
    }
}
